#include "zf_driver_file.h"
#include "zf_common_headfile.h"
#include <math.h>
#include <string.h>
 
float Dif_Effictive_Line = 45.0f;    //偏差值最大值
int16 View_Effictive_Line = 40;      //相遇点Y坐标的最大值（需要改）
 
float P_Value_L[7] = {0.1, 0.11, 0.14, 0.17f, 0.385f, 0.44f, 0.46f};
float Vague_Array[4][4] = { {0, 1, 2, 3},
                            {1, 2, 3, 4},
                            {3, 4, 5, 6},
                            {5, 6, 6, 6}};    //这个表原样抄下来
 
float f_Get_H_approximation(int16 i16_ViewH)     
{
  float H_approximation;
 
  if (i16_ViewH < 0)
  {
    i16_ViewH = 0;
  }
 
  H_approximation = ((float)i16_ViewH * 3.0f / (float)View_Effictive_Line);    //*3.0是为了将结果放大三倍
 
  return H_approximation;
}
 
float f_Get_E_approximation(float i16_E)
{
  float E_approximation;
 
  if (i16_E < 0)
  {
    i16_E = -i16_E;
  }
 
  E_approximation = (float)i16_E * 40.0f * 3.0f/ Dif_Effictive_Line;    //*3.0与上述同理，还多乘了个四十，与Dif_Effictive_Line 的值对应上
 
  return E_approximation;
}
 
int16 Off_Line = 0;
int16 Now_Off_Line = 0;    //用于滤波
int16 Last_Off_Line = 0;    //用于滤波
 
// 第一个参数输入相遇点Y坐标
// 第二个参数输入归一化后的偏差值
float Get_P(int16 off_line, float dif_value)
{
    Last_Off_Line = Now_Off_Line;
    Now_Off_Line = off_line;
 
    if(((Now_Off_Line - Last_Off_Line) >= 35) || ((Now_Off_Line - Last_Off_Line) <= -35) || Now_Off_Line >= 65)    //需要改
    {
        Off_Line = Last_Off_Line;
    }
    else
    {
        Off_Line = (int16)(0.3f * (float)(Now_Off_Line) + 0.7f * (float)(Last_Off_Line));
    }
    Off_Line = (int16)(0.3f * (float)(Now_Off_Line) + 0.7f * (float)(Last_Off_Line));
    //下面这部分代入几个数据，结合整段代码计算几遍即可理解，只看的话比较吃力
    float VH = f_Get_H_approximation(off_line - 2);
    float VE = f_Get_E_approximation(dif_value);
    float X2Y = 0;
    float X1Y = 0;
    float Y2X = 0;
    float Y1X = 0;
 
    int8 VH1 = (int)VH;
    if (VH1 > VH)
    {
      VH--;
    }
    int8 VH2 = VH1 + 1;
 
    int8 VE1 = (int8)VE;
    if (VE1 > VE)
    {
      VE1--;
    }
    int8 VE2 = VE1 + 1;
 
    if (VH1 > 3)
    {
      VH1 = 3;
    }
 
    if (VH2 > 3)
    {
      VH2 = 3;
    }
 
    if (VE1 > 3)
    {
      VE1 = 3;
    }
 
    if (VE2 > 3)
    {
      VE2 = 3;
    }
 
    X2Y = (Vague_Array[VH1][VE2] - Vague_Array[VH1][VE1]) * (VE - VE1) + Vague_Array[VH1][VE1];
 
    X1Y = (Vague_Array[VH2][VE2] - Vague_Array[VH2][VE1]) * (VE - VE1) + Vague_Array[VH2][VE1];
 
    Y2X = (Vague_Array[VH2][VE1] - Vague_Array[VH1][VE1]) * (VH - VH1) + Vague_Array[VH1][VE1];
 
    Y1X = (Vague_Array[VH2][VE2] - Vague_Array[VH1][VE2]) * (VH - VH1) + Vague_Array[VH1][VE2];
 
    float P_approximation = (X2Y + X1Y + Y2X + Y1X) / 4.0;
 
    int8 P1 = (int8)P_approximation;
    if (P1 > P_approximation)
    {
      P1--;
    }
    int8 P2 = P1 + 1;
    return (P_Value_L[P2] - P_Value_L[P1]) * (P_approximation - P1) + P_Value_L[P1]; //返回p值
}


const int rule[7][7]=
{
	{ 6 , 5 , 4 , 3 , 2 , 1 , 0},//0
	{ 5 , 4 , 3 , 2 , 1 , 0 , 1},//1
	{ 4 , 3 , 2 , 1 , 0 , 1 , 2},//2
	{ 3 , 2 , 1 , 0 , 1 , 2 , 3},//3
	{ 2 , 1 , 0 , 1 , 2 , 3 , 4},//4
	{ 1 , 0 , 1 , 2 , 3 , 4 , 5},//5
	{ 0 , 1 , 2 , 3 , 4 , 5 , 6},//6
};//模糊规则表,不改
 

float Fuzzy(float E,float EC)
{
 
    // /*输入量P语言值特征点*/
    float EFF[7]={-45,-30,-15,0,15,30,45};//要改，最大偏差值分三段
    /*输入量D语言值特征点*/
    float DFF[7]={-45,-10,-5,0,5,10,45};
    /*输出量U语言值特征点(根据赛道类型选择不同的输出值)float UFF[7]={0.13,0.201,0.272,0.323,0.334,0.355,0.369};*/
    float UFF[7]={0.11,0.181,0.272,0.323,0.334,0.355,0.369};

    float U=0;  /*偏差,偏差微分以及输出值的精确量*/
    float PF[2]={0},DF[2]={0},UF[4]={0};
    /*偏差,偏差微分以及输出值的隶属度*/
    int Pn=0,Dn=0,Un[4]={0};
    float t1=0,t2=0,t3=0,t4=0,temp1=0,temp2=0;
    /*隶属度的确定*/
    /*根据PD的指定语言值获得有效隶属度*/
    if(E>EFF[0] && E<EFF[6])
    {
        if(E<=EFF[1])
        {
            Pn=-2;
            PF[0]=(EFF[1]-E)/(EFF[1]-EFF[0]);
        }
        else if(E<=EFF[2])
        {
            Pn=-1;
            PF[0]=(EFF[2]-E)/(EFF[2]-EFF[1]);
        }
        else if(E<=EFF[3])
        {
            Pn=0;
            PF[0]=(EFF[3]-E)/(EFF[3]-EFF[2]);
        }
        else if(E<=EFF[4])
        {
            Pn=1;
            PF[0]=(EFF[4]-E)/(EFF[4]-EFF[3]);
        }
        else if(E<=EFF[5])
        {
            Pn=2;
            PF[0]=(EFF[5]-E)/(EFF[5]-EFF[4]);
        }
        else if(E<=EFF[6])
        {
            Pn=3;
            PF[0]=(EFF[6]-E)/(EFF[6]-EFF[5]);
        }
    }
 
    else if(E<=EFF[0])
    {
        Pn=-2;
        PF[0]=1;
    }
    else if(E>=EFF[6])
    {
        Pn=3;
        PF[0]=0;
    }
 
    PF[1]=1-PF[0];
 
 
    //判断D的隶属度
    if(EC>DFF[0]&&EC<DFF[6])
    {
        if(EC<=DFF[1])
        {
            Dn=-2;
            DF[0]=(DFF[1]-EC)/(DFF[1]-DFF[0]);
        }
        else if(EC<=DFF[2])
        {
            Dn=-1;
            DF[0]=(DFF[2]-EC)/(DFF[2]-DFF[1]);
        }
        else if(EC<=DFF[3])
        {
            Dn=0;
            DF[0]=(DFF[3]-EC)/(DFF[3]-DFF[2]);
        }
        else if(EC<=DFF[4])
        {
            Dn=1;
            DF[0]=(DFF[4]-EC)/(DFF[4]-DFF[3]);
        }
        else if(EC<=DFF[5])
        {
            Dn=2;
            DF[0]=(DFF[5]-EC)/(DFF[5]-DFF[4]);
        }
        else if(EC<=DFF[6])
        {
            Dn=3;
            DF[0]=(DFF[6]-EC)/(DFF[6]-DFF[5]);
        }
    }
    //不在给定的区间内
    else if (EC<=DFF[0])
    {
        Dn=-2;
        DF[0]=1;
    }
    else if(EC>=DFF[6])
    {
        Dn=3;
        DF[0]=0;
    }
 
    DF[1]=1-DF[0];
 
    /*使用误差范围优化后的规则表rule[7][7]*/
    /*输出值使用13个隶属函数,中心值由UFF[7]指定*/
    /*一般都是四个规则有效*/
    Un[0]=rule[Pn+2][Dn+2];
    Un[1]=rule[Pn+3][Dn+2];
    Un[2]=rule[Pn+2][Dn+3];
    Un[3]=rule[Pn+3][Dn+3];
 
    if(PF[0]<=DF[0])    //求小
        UF[0]=PF[0];
    else
        UF[0]=DF[0];
    if(PF[1]<=DF[0])
        UF[1]=PF[1];
    else
        UF[1]=DF[0];
    if(PF[0]<=DF[1])
        UF[2]=PF[0];
    else
        UF[2]=DF[1];
    if(PF[1]<=DF[1])
        UF[3]=PF[1];
    else
        UF[3]=DF[1];
    /*同隶属函数输出语言值求大*/
    if(Un[0]==Un[1])
    {
        if(UF[0]>UF[1])
            UF[1]=0;
        else
            UF[0]=0;
    }
    if(Un[0]==Un[2])
    {
        if(UF[0]>UF[2])
            UF[2]=0;
        else
            UF[0]=0;
    }
    if(Un[0]==Un[3])
    {
        if(UF[0]>UF[3])
            UF[3]=0;
        else
            UF[0]=0;
    }
    if(Un[1]==Un[2])
    {
        if(UF[1]>UF[2])
            UF[2]=0;
        else
            UF[1]=0;
    }
    if(Un[1]==Un[3])
    {
        if(UF[1]>UF[3])
            UF[3]=0;
        else
            UF[1]=0;
    }
    if(Un[2]==Un[3])
    {
        if(UF[2]>UF[3])
            UF[3]=0;
        else
            UF[2]=0;
    }
    t1=UF[0]*UFF[Un[0]];
    t2=UF[1]*UFF[Un[1]];
    t3=UF[2]*UFF[Un[2]];
    t4=UF[3]*UFF[Un[3]];
    temp1=t1+t2+t3+t4;
    temp2=UF[0]+UF[1]+UF[2]+UF[3];//模糊量输出
    U=temp1/temp2;
    return U;
}

// 模糊规则表 (基础版本)
const int base_rule[7][7] = {
    {5, 4, 4, 3, 2, 1, 0},  // 更平滑的过渡
    {4, 4, 3, 2, 1, 0, 1},
    {4, 3, 2, 1, 0, 1, 2},
    {3, 2, 1, 1, 1, 2, 3},
    {2, 1, 0, 1, 2, 3, 4},
    {1, 0, 1, 2, 3, 4, 5},
    {0, 1, 2, 3, 4, 5, 6}
};

// 线性插值函数
float lerp(float a, float b, float t) {
    return a + t*(b-a);
}

float Fuzzy2(float E, float EC) {
    // ===================== 信号预处理 =====================
    // 二阶低通滤波
    #define FILTER_ALPHA 0.4f
    static float e_history[3] = {0};
    static float ec_history[3] = {0};
    
    e_history[2] = e_history[1];
    e_history[1] = e_history[0];
    e_history[0] = E;
    
    ec_history[2] = ec_history[1];
    ec_history[1] = ec_history[0];
    ec_history[0] = EC;
    
    E = FILTER_ALPHA * e_history[0] + 
        (1-FILTER_ALPHA)*FILTER_ALPHA * e_history[1] + 
        (1-FILTER_ALPHA)*(1-FILTER_ALPHA) * e_history[2];
    
    EC = FILTER_ALPHA * ec_history[0] + 
         (1-FILTER_ALPHA)*FILTER_ALPHA * ec_history[1] + 
         (1-FILTER_ALPHA)*(1-FILTER_ALPHA) * ec_history[2];
    
    // ===================== 状态检测 =====================
    static float lastE = 0;
    static int transitionState = 0; // 0:直道 1:入弯 2:出弯
    static int oscillationCount = 0;
    
    // 方向切换检测
    if(lastE * E < 0 && fabs(E) > 5.0f) { // 符号变化且超过阈值
        if(E < 0) {
            transitionState = 1; // 入弯
        } else {
            transitionState = 2; // 出弯
            oscillationCount = 0; // 重置震荡计数
        }
    } 
    // 直道检测
    else if(fabs(E) < 8.0f && fabs(EC) < 4.0f) {
        transitionState = 0; // 直道
    }
    lastE = E;
    
    // ===================== 参数配置 =====================
    // 输入特征点 (根据状态动态调整)
    float EFF[7], DFF[7];
    
    // 直道模式 - 扩大零区提高稳定性
    if(transitionState == 0) {
        float EFF_base[7] = {-45, -35, -20, 0, 20, 35, 45};
        float DFF_base[7] = {-27, -15, -8, 0, 8, 15, 27};
        memcpy(EFF, EFF_base, sizeof(EFF_base));
        memcpy(DFF, DFF_base, sizeof(DFF_base));
    }
    // 入弯模式 - 提高灵敏度
    else if(transitionState == 1) {
        float EFF_base[7] = {-45, -30, -15, 0, 15, 30, 45};
        float DFF_base[7] = {-27, -12, -6, 0, 6, 12, 27};
        memcpy(EFF, EFF_base, sizeof(EFF_base));
        memcpy(DFF, DFF_base, sizeof(DFF_base));
    }
    // 出弯模式 - 扩大过渡区抑制震荡
    else {
        float EFF_base[7] = {-45, -35, -25, 0, 25, 35, 45};
        float DFF_base[7] = {-27, -18, -9, 0, 9, 18, 27};
        memcpy(EFF, EFF_base, sizeof(EFF_base));
        memcpy(DFF, DFF_base, sizeof(DFF_base));
    }
    
    // 输出特征点 (速度自适应)
    float UFF[7];
    const float speed_ratio = (Speed_Target - 400) / 150.0f; // 400-550映射到0-1
    
    // 基础值设置
    UFF[0] = lerp(0.10f, 0.12f, speed_ratio);  // NB
    UFF[1] = lerp(0.15f, 0.18f, speed_ratio);  // NM
    UFF[2] = lerp(0.25f, 0.30f, speed_ratio);  // NS
    UFF[3] = lerp(0.32f, 0.40f, speed_ratio);  // ZO
    UFF[4] = lerp(0.38f, 0.48f, speed_ratio);  // PS
    UFF[5] = lerp(0.42f, 0.58f, speed_ratio);  // PM
    UFF[6] = lerp(0.66f, 0.70f, speed_ratio);  // PB
    
    // 死区增强 (直道专用)
    if(transitionState == 0) {
        if(fabs(E) < 3.0f) E = 0;
        if(fabs(EC) < 2.0f) EC = 0;
        UFF[3] *= 0.8f;  // 零区输出降低20%
    }
    // 出弯模式特殊处理
    else if(transitionState == 2) {
        EC *= 0.7f;  // 降低微分增益30%
        UFF[3] *= 1.2f; // 提高零区稳定性
    }
    
    // ===================== 模糊推理 =====================
    float PF[2] = {0}, DF[2] = {0}, UF[4] = {0};
    int Pn = 0, Dn = 0, Un[4] = {0};
    float t1 = 0, t2 = 0, t3 = 0, t4 = 0, temp1 = 0, temp2 = 0;
    
    /* 确定E的隶属度 */
    if(E > EFF[0] && E < EFF[6]) {
        if(E <= EFF[1]) {
            Pn = -2;
            PF[0] = (EFF[1] - E) / (EFF[1] - EFF[0]);
        } else if(E <= EFF[2]) {
            Pn = -1;
            PF[0] = (EFF[2] - E) / (EFF[2] - EFF[1]);
        } else if(E <= EFF[3]) {
            Pn = 0;
            PF[0] = (EFF[3] - E) / (EFF[3] - EFF[2]);
        } else if(E <= EFF[4]) {
            Pn = 1;
            PF[0] = (EFF[4] - E) / (EFF[4] - EFF[3]);
        } else if(E <= EFF[5]) {
            Pn = 2;
            PF[0] = (EFF[5] - E) / (EFF[5] - EFF[4]);
        } else if(E <= EFF[6]) {
            Pn = 3;
            PF[0] = (EFF[6] - E) / (EFF[6] - EFF[5]);
        }
    } else if(E <= EFF[0]) {
        Pn = -2;
        PF[0] = 1;
    } else if(E >= EFF[6]) {
        Pn = 3;
        PF[0] = 0;
    }
    PF[1] = 1 - PF[0];
    
    /* 确定EC的隶属度 */
    if(EC > DFF[0] && EC < DFF[6]) {
        if(EC <= DFF[1]) {
            Dn = -2;
            DF[0] = (DFF[1] - EC) / (DFF[1] - DFF[0]);
        } else if(EC <= DFF[2]) {
            Dn = -1;
            DF[0] = (DFF[2] - EC) / (DFF[2] - DFF[1]);
        } else if(EC <= DFF[3]) {
            Dn = 0;
            DF[0] = (DFF[3] - EC) / (DFF[3] - DFF[2]);
        } else if(EC <= DFF[4]) {
            Dn = 1;
            DF[0] = (DFF[4] - EC) / (DFF[4] - DFF[3]);
        } else if(EC <= DFF[5]) {
            Dn = 2;
            DF[0] = (DFF[5] - EC) / (DFF[5] - DFF[4]);
        } else if(EC <= DFF[6]) {
            Dn = 3;
            DF[0] = (DFF[6] - EC) / (DFF[6] - DFF[5]);
        }
    } else if(EC <= DFF[0]) {
        Dn = -2;
        DF[0] = 1;
    } else if(EC >= DFF[6]) {
        Dn = 3;
        DF[0] = 0;
    }
    DF[1] = 1 - DF[0];
    
    // ===================== 动态规则应用 =====================
    int dynamic_rule[7][7];
    
    // 直道模式 - 平滑规则
    if(transitionState == 0) {
        const int smooth_rule[7][7] = {
            {5, 4, 4, 3, 2, 1, 0},
            {4, 4, 3, 2, 1, 0, 1},
            {4, 3, 2, 1, 0, 1, 2},
            {3, 2, 1, 1, 1, 2, 3},
            {2, 1, 0, 1, 2, 3, 4},
            {1, 0, 1, 2, 3, 4, 5},
            {0, 1, 2, 3, 4, 5, 6}
        };
        memcpy(dynamic_rule, smooth_rule, sizeof(smooth_rule));
    }
    // 入弯模式 - 积极规则
    else if(transitionState == 1) {
        const int active_rule[7][7] = {
            {5, 4, 3, 2, 1, 0, 0},
            {4, 3, 2, 1, 0, 0, 1},
            {3, 2, 1, 0, 0, 1, 2},
            {2, 1, 0, 0, 1, 2, 3},
            {1, 0, 0, 1, 2, 3, 4},
            {0, 0, 1, 2, 3, 4, 5},
            {0, 1, 2, 3, 4, 5, 6}
        };
        memcpy(dynamic_rule, active_rule, sizeof(active_rule));
    }
    // 出弯模式 - 保守规则
    else {
        const int conservative_rule[7][7] = {
            {6, 5, 4, 3, 2, 1, 0},
            {5, 4, 3, 2, 1, 0, 1},
            {4, 3, 2, 1, 0, 1, 2},
            {3, 2, 1, 2, 2, 3, 3},
            {2, 1, 0, 1, 2, 3, 4},
            {1, 0, 1, 2, 3, 4, 5},
            {0, 1, 2, 3, 4, 5, 6}
        };
        memcpy(dynamic_rule, conservative_rule, sizeof(conservative_rule));
    }
    
    /* 确定有效规则 */
    Un[0] = dynamic_rule[Pn+2][Dn+2];
    Un[1] = dynamic_rule[Pn+3][Dn+2];
    Un[2] = dynamic_rule[Pn+2][Dn+3];
    Un[3] = dynamic_rule[Pn+3][Dn+3];
    
    if(PF[0] <= DF[0])
        UF[0] = PF[0];
    else
        UF[0] = DF[0];
        
    if(PF[1] <= DF[0])
        UF[1] = PF[1];
    else
        UF[1] = DF[0];
        
    if(PF[0] <= DF[1])
        UF[2] = PF[0];
    else
        UF[2] = DF[1];
        
    if(PF[1] <= DF[1])
        UF[3] = PF[1];
    else
        UF[3] = DF[1];
        
    /* 同隶属函数输出语言值求大 */
    if(Un[0] == Un[1]) {
        if(UF[0] > UF[1])
            UF[1] = 0;
        else
            UF[0] = 0;
    }
    if(Un[0] == Un[2]) {
        if(UF[0] > UF[2])
            UF[2] = 0;
        else
            UF[0] = 0;
    }
    if(Un[0] == Un[3]) {
        if(UF[0] > UF[3])
            UF[3] = 0;
        else
            UF[0] = 0;
    }
    if(Un[1] == Un[2]) {
        if(UF[1] > UF[2])
            UF[2] = 0;
        else
            UF[1] = 0;
    }
    if(Un[1] == Un[3]) {
        if(UF[1] > UF[3])
            UF[3] = 0;
        else
            UF[1] = 0;
    }
    if(Un[2] == Un[3]) {
        if(UF[2] > UF[3])
            UF[3] = 0;
        else
            UF[2] = 0;
    }
    
    t1 = UF[0] * UFF[Un[0]];
    t2 = UF[1] * UFF[Un[1]];
    t3 = UF[2] * UFF[Un[2]];
    t4 = UF[3] * UFF[Un[3]];
    temp1 = t1 + t2 + t3 + t4;
    temp2 = UF[0] + UF[1] + UF[2] + UF[3];
    float U = temp1 / temp2;
    
    // ===================== 震荡抑制 =====================
    static float lastU = 0;
    static float lastDelta = 0;
    float currentDelta = U - lastU;
    
    // 震荡检测 (符号交替变化)
    if(currentDelta * lastDelta < 0 && fabs(currentDelta) > 0.05f) {
        oscillationCount++;
    } else {
        oscillationCount = (oscillationCount > 0) ? oscillationCount - 1 : 0;
    }
    
    // 震荡处理
    if(oscillationCount > 2) {
        // 强滤波 + 输出限制
        U = lastU * 0.7f + U * 0.3f;
        
        // 根据状态限制最大输出
        float maxOutput = (transitionState == 1) ? 0.65f : 0.45f;
        if(fabs(U) > maxOutput) {
            U = copysign(maxOutput, U);
        }
        
        oscillationCount = 2; // 保持抑制状态
    }
    
    lastDelta = currentDelta;
    lastU = U;
    
    // ===================== 调试输出 =====================
    #ifdef DEBUG_MODE
    printf("State:%d E:%.1f EC:%.1f U:%.3f OSC:%d\n", 
           transitionState, E, EC, U, oscillationCount);
    #endif
    
    return U;
}
